/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 * Modified function input and used only some functions
 **/
#include "piecewise_linear_generator.h"

#include "active_set_qp_solver.h"

// NOTICE: the first point is kept at 0.0
PiecewiseLinearGenerator::PiecewiseLinearGenerator(
    const uint32_t num_of_segments, const double unit_segment)
    : num_of_segments_(num_of_segments),
      unit_segment_(unit_segment),
      total_t_(num_of_segments * unit_segment),
      constraint_(num_of_segments, unit_segment),
      kernel_(num_of_segments, unit_segment)
{
    // CHECK_GE(num_of_segments, 3);
}

PiecewiseLinearConstraint *PiecewiseLinearGenerator::mutable_constraint()
{
    return &constraint_;
}

PiecewiseLinearKernel *PiecewiseLinearGenerator::mutable_kernel()
{
    return &kernel_;
}

bool PiecewiseLinearGenerator::Solve()
{
    const Eigen::MatrixXd &kernel_matrix = kernel_.kernel_matrix();
    const Eigen::MatrixXd &offset = kernel_.offset_matrix();

    const Eigen::MatrixXd &inequality_constraint_matrix =
        constraint_.inequality_constraint_matrix();
    const Eigen::MatrixXd &inequality_constraint_boundary =
        constraint_.inequality_constraint_boundary();

    const Eigen::MatrixXd &equality_constraint_matrix =
        constraint_.equality_constraint_matrix();
    const Eigen::MatrixXd &equality_constraint_boundary =
        constraint_.equality_constraint_boundary();

    qp_solver_.reset(new ActiveSetQpSolver(
        kernel_matrix, offset, inequality_constraint_matrix,
        inequality_constraint_boundary, equality_constraint_matrix,
        equality_constraint_boundary));

    qp_solver_->EnableCholeskyRefactorisation(1);
    qp_solver_->set_pos_definite_hessian();

    if (!qp_solver_->Solve())
    {
        return false;
    }
    const Eigen::MatrixXd solved_params = qp_solver_->params();
    return true;
}
